package mge.graph 
{
  import flash.geom.Point;
	/**
   * ...
   * @author ivan
   */
  public class AStar 
  {
    
    public function AStar() 
    {
      
    }
    
    public function findPath(start:Node, goal:Node):Vector.<Node>
    {
      var startNode:EvaluatedNode = new EvaluatedNode(start);
      startNode.g = 0;
      startNode.h = heuristicEstimate(start, goal);
      startNode.f = startNode.h;
      openSet.push(startNode);
      
      while (openSet.length)
      {
        openSet.sort(lowestF);
        
        var x:EvaluatedNode = openSet[0];
        if (x.node.name == goal.name)
        {
          var v:Vector.<EvaluatedNode> = new Vector.<EvaluatedNode>();
          
          while (x.cameFrom)
          {
            v.push(x);
            x = x.cameFrom;
          }
          v.reverse();
          
          var ans:Vector.<Node> = new Vector.<Node>();
          ans.push(start);
          for (i = 0; i < v.length; i++)
            ans.push(v[i].node);
            
          return ans;
        }
        
        openSet.splice(openSet.indexOf(x), 1);
        closedSet.push(x);
        
        var neighbors:Vector.<EvaluatedNode> = neighbors(x);
        
        function sameName(item:EvaluatedNode, index:int, vector:Vector.<EvaluatedNode>):Boolean
        {
          return (item.node.name == y.node.name);
        }
        
        for (var i:int = 0; i < neighbors.length; i++) 
        {
          var y:EvaluatedNode = neighbors[i];
          if (closedSet.some(sameName, this))
            continue;
          
          var tentative_g:Number = x.g + distance(x, y);
          var tentativeIsBetter:Boolean;
          
          if (!openSet.some(sameName, this))
          {
            openSet.push(y);
            tentativeIsBetter = true;
          }
          else if (tentative_g < y.g)
            tentativeIsBetter = true;
          else
            tentativeIsBetter = false;
          
          if (tentativeIsBetter)
          {
             y.cameFrom = x;
             y.g = tentative_g;
             y.h = heuristicEstimate(y.node, goal);
             y.f = y.f + y.h;
          }
        }
        
      }
      
      return null;
    }
    
    private function neighbors(node:EvaluatedNode):Vector.<EvaluatedNode>
    {
      var v:Vector.<EvaluatedNode> = new Vector.<EvaluatedNode>();
      
      for (var i:int = 0; i < graph.edges.length; i++)
      {
        var edge:Edge = graph.edges[i];
        if (edge.source.name == node.node.name)
          v.push(new EvaluatedNode(edge.target));
        if (edge.target.name == node.node.name)
          v.push(new EvaluatedNode(edge.source));
      }
      
      return v;
    }
    
    private function heuristicEstimate(start:Node, goal:Node):Number
    {
      var p1:Point = new Point(start.x, start.y);
      var p2:Point = new Point(goal.x, goal.y);
      return p1.subtract(p2).length;
    }
    
    private function distance(a:EvaluatedNode, b:EvaluatedNode):Number
    {
      var p1:Point = new Point(a.node.x, a.node.y);
      var p2:Point = new Point(b.node.x, b.node.y);
      return p1.subtract(p2).length;
    }
    
    private function lowestF(a:EvaluatedNode, b:EvaluatedNode):Number
    {
      if (a.f < b.f) return -1;
      else if (a.f > b.f) return +1;
      return 0;
    }
    
    public var graph:Graph;
    
    private var openSet:Vector.<EvaluatedNode> = new Vector.<EvaluatedNode>();
    private var closedSet:Vector.<EvaluatedNode> = new Vector.<EvaluatedNode>();
    
  }

}

import mge.graph.Node;

internal class EvaluatedNode
{
  public function EvaluatedNode(node:Node)
  {
    this.node = node;
  }
  
  public var h:Number = 0;
  public var g:Number = Number.MAX_VALUE;
  public var f:Number = 0;
  
  public var node:Node;
  
  public var cameFrom:EvaluatedNode;
  
};